/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#include "DiscreteObject.h"

#include <numeric>
#include <algorithm>
#include <iostream>

#include "Jano/NLSaturation.h"

//Test:
#include <fstream>
#include "RmnkSolver.h"

//#include "SimInterface/ICommunicationBridge.h"
#include "Utils.h"

// Przestrzeń nazw
namespace RafMat
{
namespace SimEngine
{

using namespace std;
using namespace SimInterface;

const string DiscreteObject::CONF_MAX_ADEGREE = "max_adegree";
const string DiscreteObject::CONF_MAX_BDEGREE = "max_bdegree";
const string DiscreteObject::CONF_MAX_CDEGREE = "max_cdegree";
const string DiscreteObject::CONF_MAX_AGDEGREE = "max_agdegree";
const string DiscreteObject::CONF_MAX_D = "max_d";
const string DiscreteObject::CONF_MAX_DELAY = "max_delay";
//const string DiscreteObject::CONF_OUTNL_ENABLED = "out_nl_enabled";
//const string DiscreteObject::CONF_OUTNL_TYPE = "out_nl_type";

/* Konstruktor */
DiscreteObject::DiscreteObject(const ISimulator &simulator,
                               unsigned int aRank,
                               unsigned int bRank,
                               unsigned int cRank,
                               unsigned int agRank,
                               unsigned int d,
                               unsigned int delay)
    : SISOModel(simulator, "ARXModel"),
      mc_defaultParams(DiscreteObjectParamsSPtr(new DiscreteObjectParams(*this))),
      m_aMaxRank(aRank), m_bMaxRank(bRank),m_cMaxRank(cRank), m_agMaxRank(agRank), m_maxD(d), m_maxDelay(delay),
      m_noiseEnabled(true)//, m_nonlinearityEnabled(false)
{
    initialize();
}

void DiscreteObject::initialize()
{
    resizeMemories();

    // Domyślnie dajemy prosty model obiektu typu bypass (wzmacniacz k=1).
    // switchTime dajemy -2, jako wartość zarezerwowaną i zapewniającą najwyższą
    // pozycję w liście parametrów.
    DiscreteObjectParamsSPtr simpleK(new DiscreteObjectParams(*this, 1, 1, 0,1,0, 0, -2));
    insertParameters(simpleK);

    //setNonlinearity(NonlinearityNLSaturation);
}

void DiscreteObject::resizeMemories()
{
    // Pamięć próbek wyjściowych obejmuje próbki: y(i-1)...y*(i-dA)
    m_outmem.resize(m_aMaxRank);

    // Pamięć próbek wejściowych obejmują próbki: u(i-n)...u(i-n-dB)
    // trzeba więc pamiętać n+dB+1 wartości (włącznie z u(i)...u(i-n+1).
    m_inmem.resize(m_bMaxRank + m_maxDelay + 1);

    // Pamięć próbek szumu obejmuje próbki: e(i-1)...e*(i-dC)       // A nie od e(i) ??? [Mateusz]
    m_emem.resize(m_cMaxRank);

    //Pamiec na wyjscie DD i wejscie - tylko dwie probki bo to jest delta i nie ma zaleznosci od poprzednich probek
    m_ddoutmem.resize(m_agMaxRank);

}

//void DiscreteObject::setNonlinearity(NonlinearityType nlType)
//{
//    switch(nlType)
//    {
//    case NonlinearityNone:
//        break;
//        m_nonlinearityEnabled = false;
//        m_outputNL.reset();
//    case NonlinearityNLSaturation:
//        m_nonlinearityEnabled = true;
//        m_outputNL.reset(new NLSaturation(getSimulator()));
//        break;
//    }
//}

void DiscreteObject::insertParameters(DiscreteObjectParamsSPtr dop)
{
    if(dop->getADegree() > m_aMaxRank
            || dop->getBDegree() > m_bMaxRank
            || dop->getCDegree() > m_cMaxRank
            || dop->getAgDegree() > m_agMaxRank
            || dop->getD() > m_maxD
            || dop->getDelay() > m_maxDelay)
        //throw invalid_argument("Given parameters have higher order than supposed.");
    {
        //ICommunicationBridge::getInstance()->error("Given parameters have higher order than supposed.");
        /** \todo Tu powinien być rzucony wyjątek. Inaczej GUI się nie dowie że coś poszło nie tak. */
        return;
    }

    m_modelParams.push_back(dop);
}

void DiscreteObject::insertParameters(Polynomial a, Polynomial b, Polynomial c, Polynomial ag, double d ,unsigned int delay, int switchTime)
{
    DiscreteObjectParamsSPtr tmp(new DiscreteObjectParams(*this, a, b, c,ag,d, delay, switchTime));
    insertParameters(tmp);
}

IConfigurableSPtr DiscreteObject::insertParameters(vector<double> a, vector<double> b,
                                      vector<double> c, vector<double> ag, double d , unsigned int delay, int switchTime)
{
    DiscreteObjectParamsSPtr tmp(new DiscreteObjectParams(*this, Polynomial(a), Polynomial(b), Polynomial(c),Polynomial(ag), d ,delay, switchTime));
    insertParameters(tmp);

    // Konwersja do klasy bazowej
    return boost::static_pointer_cast<IConfigurable>(tmp);
}

void DiscreteObject::deleteParameters(IConfigurableSPtr paramSet)
{
    // Usunięcie obiektu (trzeba rzutować dynamicznie)
    // Jeśli nie znajdzie takiego wskaźnika to nic się nie dzieje.
    remove(m_modelParams.begin(), m_modelParams.end(),
           boost::dynamic_pointer_cast<DiscreteObjectParams>(paramSet));
}

vector<IConfigurableSPtr> DiscreteObject::getParameters() const
{
    vector<IConfigurableSPtr> tmp;
    vector<DiscreteObjectParamsSPtr>::const_iterator it;

    it = m_modelParams.begin();
    // Jeśli są jakieś parametry inne niż domyślny, to ten domyślny pomijamy:
    if(m_modelParams.size() > 1) ++it;

    // Zwracana jest zawsze kopia wektora z przekonwertowanymi wskaźnikami
    for(; it != m_modelParams.end(); ++it)
    {
        tmp.push_back(boost::static_pointer_cast<IConfigurable>(*it));
    }
    return tmp;
}

// Linkowanie wewnętrzne.
namespace{
/**
    Porównuje dwa obiekty DiscreteObjectParams.
    Funkcja używana jako predykat w unique oraz komparator w sort.
*/
bool cmpDo(DiscreteObjectParamsSPtr lhs, DiscreteObjectParamsSPtr rhs)
{
    return lhs->getSwitchTime() < rhs->getSwitchTime();
}
}

/**
    Funkcja aktualizuje listę parametrów po ich edycji.
    Aktualizacja ta polega na posortowaniu zestawów parametrów modelu niestacjonarnego
    według momentów zmian.
    Następnie usuwane są powtarzające się parametry dla tych samych czasów (GUI prawdopodobnie
    będzie musiało odświeżyć stan modelu, żeby było wiadomo że faktycznie coś się usunęło).
*/
void DiscreteObject::updateParameters()
{
    sort(m_modelParams.begin(), m_modelParams.end(), cmpDo);
    unique(m_modelParams.begin(), m_modelParams.end(), cmpDo);
    m_currentMPIndex = 0;
}

DiscreteObjectParamsSPtr DiscreteObject::getNextParams(bool& changed)
{    
    unsigned int index = m_currentMPIndex;
    changed = false;

    if(m_modelParams.size() == 0)
        return mc_defaultParams;

    if(index >= m_modelParams.size() - 1)
        return m_modelParams.back();

    DiscreteObjectParamsSPtr tmp = m_modelParams[index+1];

    int swTime = tmp->getSwitchTime();
    if(swTime >= 0 && static_cast<unsigned int>(swTime) > getSimulator().currentSampleNum())
    {
        return m_modelParams[index];
    }
    else
    {
        changed = true;
        return m_modelParams[++m_currentMPIndex];
    }
}

double DiscreteObject::simulateStep(double input)
{
    // TEST:
    //static RmnkSolver rmnk(1, 1, 0, 0.9, 0.9, 10);

    // Najpierw wkładamy próbkę do pamięci:
    m_inmem.push_front(input);
    m_inmem.pop_back();             // usunięcie ostatniej próbki.

    // Aktualny zestaw parametrów modelu:
    bool paramsWereSwitched;
    DiscreteObjectParamsSPtr params = getNextParams(paramsWereSwitched);

    // Jeśli model się zmienił, trzeba zaktualizować generator szumu.
    //if(paramsWereSwitched)   // wałek gdy użytkownik zmieni ustawienia w trakcie
        m_noiseGen.setGain(params->getNoiseGain());

    // Definicja iteratorów:
    vector<double>::iterator
            apoly_it = ++params->abegin(),                   // zaczynamy od a1 nie a0
            apoly_last_it = params->aend(),
            bpoly_it = params->bbegin(),                     // tu zaczynamy od b0
            bpoly_last_it = params->bend(),
            cpoly_it = params->cbegin(),                     // tu zaczynamy od c0
            cpoly_last_it = params->cend(),
            agpoly_it = ++params->agbegin(),                     // tu zaczynamy od ag0
            agpoly_last_it = params->agend();
    deque<double>::iterator
            inmem_it = m_inmem.begin() + params->getDelay(), // zaczynamy od u(i-delay)
            outmem_it = m_outmem.begin(),                   // zaczynamy od y(i-1)
            emem_it = m_emem.begin(),
            ddoutmem_it = m_ddoutmem.begin();

    // Obliczenie sumy częściowej b0*u(i-n)+b1*u(i-n-1)+...
    double tmpSum = 0;
    double tmpDD = 0;
    tmpSum = inner_product(bpoly_it, bpoly_last_it, inmem_it, tmpSum, plus<double>(), multiplies<double>());

    // Dodanie do sumy wyrazu -a1*y(i-1)-a2*y(i-2)-...
    tmpSum = inner_product(apoly_it, apoly_last_it, outmem_it, tmpSum, minus<double>(), multiplies<double>());

    // Dodanie składowej szumu losowego.
    if(m_noiseEnabled)
    {
        // Najpierw wkładamy próbkę do pamięci:
        m_emem.push_front(m_noiseGen.getNextValue());
        m_emem.pop_back();             // usunięcie ostatniej próbki.

        tmpSum = inner_product(cpoly_it, cpoly_last_it, emem_it, tmpSum, plus<double>(), multiplies<double>());
    }

    // Dodanie składowej szumu deterministycznego
    tmpDD =  inner_product(agpoly_it, agpoly_last_it, ddoutmem_it, tmpDD, minus<double>(), multiplies<double>());

    if(getSimulator().currentSampleNum() == static_cast<unsigned>(params->getSwitchTime()+1))
        tmpDD += params->getD();

    tmpSum += tmpDD;

    // Zapamietanie y
    m_outmem.push_front(tmpSum);
    m_outmem.pop_back();
    m_ddoutmem.push_front(tmpDD);
    m_ddoutmem.pop_back();

    return tmpSum;

    // Przepuść wyjście przez nieliniowość wyjściową:
//    if(m_nonlinearityEnabled)
//    {
//        return m_outputNL->simulateStep(tmpSum);
//    }
//    else
//    {
//        return tmpSum;
//    }
}

void DiscreteObject::reset()
{
    fill(m_inmem.begin(), m_inmem.end(), 0);
    fill(m_outmem.begin(), m_outmem.end(), 0);
    fill(m_emem.begin(), m_emem.end(), 0);
    fill(m_ddoutmem.begin(), m_ddoutmem.end(), 0);

    updateParameters();
}

void DiscreteObject::reset(const vector<double>& u0, const vector<double>& y0, const vector<double> &e0)
{
    typedef vector<double>::const_iterator iit;

    reset();

    iit last;
    unsigned int size;

    size = m_inmem.size();
    if(u0.size() <= size)
        last = u0.end();
    else
        last = u0.begin() + size;

    copy(u0.begin(), last, m_inmem.begin());

    size = m_outmem.size();
    if(y0.size() <= size)
        last = y0.end();
    else
        last = y0.begin() + size;

    copy(y0.begin(), last, m_outmem.begin());

    size = m_emem.size();
    if(e0.size() <= size)
        last = e0.end();
    else
        last = e0.begin() + size;

    copy(e0.begin(), last, m_emem.begin());

    //Nie mam ochoty sie bawic z tym, a to i tak sie nie przyda:)
    fill(m_ddoutmem.begin(), m_ddoutmem.end(), 0);

}

vector<double> DiscreteObject::getPastInputSamples(int num)
{
    int size = Utils::limitValue<int>(num, 0, m_inmem.size());

    return vector<double> (m_inmem.begin(), m_inmem.begin() + size);
}

//vector<double> DiscreteObject::getPastOutputSamples(int num)
//{
//
//}

//
// Interfejs IConfigurable
//

bool DiscreteObject::setParameter(const string& param, const string& value) throw(iconf_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string tvalue = Utils::trimWhiteSpaces(value);

    // Strumień danych dla łańcucha argumentu.
    istringstream istr(tvalue);

    int val;
    istr >> val;

    // Parametry max. można zmienić tylko jeśli spełnione są warunki:
    // - można zmieniać dowolnie jeśli model nie ma przypisanych parametrów oprócz domyślnych
    // - można jedynie zwiększać w przeciwnym wypadku.
    if(!istr.bad())
    {
        if(tparam == CONF_MAX_ADEGREE)
        {
            if((m_modelParams.size() > 1 && val >= m_aMaxRank) || m_modelParams.size() == 1)
            {
                m_aMaxRank = val;
                resizeMemories(); reset();
            }
            else throw iconf_error("Invalid max degree.");
        }
        else if(tparam == CONF_MAX_BDEGREE)
        {
            if((m_modelParams.size() > 1 && val >= m_bMaxRank) || m_modelParams.size() == 1)
            {
                m_bMaxRank = val;
                resizeMemories(); reset();
            }
            else throw iconf_error("Invalid max degree.");
        }
        else if(tparam == CONF_MAX_CDEGREE)
        {
            if((m_modelParams.size() > 1 && val >= m_cMaxRank) || m_modelParams.size() == 1)
            {
                m_cMaxRank = val;
                resizeMemories(); reset();
            }
            else throw iconf_error("Invalid max degree.");
        }
        else if(tparam == CONF_MAX_AGDEGREE)
        {
            if((m_modelParams.size() > 1 && val >= m_agMaxRank) || m_modelParams.size() == 1)
            {
                m_agMaxRank = val;
                resizeMemories(); reset();
            }
            else throw iconf_error("Invalid max degree.");
        }
        else if(tparam == CONF_MAX_DELAY)
        {
            if((m_modelParams.size() > 1 && val >= static_cast<int>(m_maxDelay)) || m_modelParams.size() == 1)
            {
                m_maxDelay = val;
                resizeMemories(); reset();
            }
            else throw iconf_error("Invalid max degree.");
        }
        else if(tparam == CONF_MAX_D)
        {
            if((m_modelParams.size() > 1 && val >= static_cast<int>(m_maxD)) || m_modelParams.size() == 1)
            {
                m_maxD = val;
                resizeMemories(); reset();
            }
            else throw iconf_error("Invalid max degree.");
        }
        else throw iconf_error::unrecognizedParam(*this, tparam);
    }

    if(istr.bad())
        throw iconf_error::syntaxError(*this, tvalue);

    return true;
}

std::string DiscreteObject::getParameter(const string& param) const throw(iconf_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string outStr;

    // Strumień danych dla łańcucha argumentu.
    std::ostringstream ostr(outStr);

    if(tparam == CONF_MAX_ADEGREE)
    {
        ostr << m_aMaxRank;
    }
    else if(tparam == CONF_MAX_BDEGREE)
    {
        ostr << m_bMaxRank;
    }
    else if(tparam == CONF_MAX_CDEGREE)
    {
        ostr << m_cMaxRank;
    }
    else if(tparam == CONF_MAX_AGDEGREE)
    {
        ostr << m_agMaxRank;
    }
    else if(tparam == CONF_MAX_D)
    {
        ostr << m_maxD;
    }
    else if(tparam == CONF_MAX_DELAY)
    {
        ostr << m_maxDelay;
    }
//    else if(tparam == CONF_OUTNL_ENABLED)
//    {
//        ostr << static_cast<int>(m_nonlinearityEnabled);
//    }
//    else if(tparam == CONF_OUTNL_TYPE)
//    {
//        ostr <<  m_outputNL->getType();
//    }
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    return ostr.str();
}

SimFile::ConfigWriter& DiscreteObject::dumpParameters(SimFile::ConfigWriter& o) const
{
    o.pushBlock(SimFile::ConfigWriter::BlockModel, getName());

    o.pushParam(CONF_MAX_ADEGREE, m_aMaxRank);
    o.pushParam(CONF_MAX_BDEGREE, m_bMaxRank);
    o.pushParam(CONF_MAX_CDEGREE, m_cMaxRank);
    o.pushParam(CONF_MAX_AGDEGREE, m_agMaxRank);
    o.pushParam(CONF_MAX_D, m_maxD);
    o.pushParam(CONF_MAX_DELAY, m_maxDelay);
    //o.pushParam(CONF_OUTNL_ENABLED, static_cast<int>(m_nonlinearityEnabled));
    //o.pushParam(CONF_OUTNL_TYPE, m_outputNL->getType());

    // Wyślij konfigurację obiektu nieliniowego
    //o << *m_outputNL;

    o.pushEmptyLine();

    vector<DiscreteObjectParamsSPtr>::const_iterator it;
    for(it = m_modelParams.begin(); it != m_modelParams.end(); ++it)
    {
        if((*it)->getSwitchTime() >= -1)
        {
            // Wyślij do konfiguracji zestaw parametrów.
            o << *it->get();
        }
    }

    o.closeBlock();
    return o;
}

}   // SimEngine namespace
}   // RafMat namespace
